
import lejos.robotics.subsumption.*;
import lejos.nxt.*;

/**
 * Demonstration of the Behavior subsumption classes.
 * 
 * Requires a wheeled vehicle with two independently controlled
 * motors connected to motor ports A and C, and 
 * a touchmsensor connected to sensor  port 1 and
 * an ultrasonic sensor connected to port 3;
 * 
 * @author Brian Bagnall and Lawrie Griffiths, modified by Roger Glassey
 *
 */
public class BumperCar
{

	public static void main(String[] args)
	{
		Motor.A.setSpeed(400);
		Motor.C.setSpeed(400);
		Behavior b1 = new DriveForward();
		Behavior b2 = new DetectWall();
		Behavior b3 = new Exit();
		Behavior[] behaviorList =
		{
				b1, b2, b3
		};
		Arbitrator arbitrator = new Arbitrator(behaviorList);
		LCD.drawString("Bumper Car",0,1);
		Button.waitForPress();
		arbitrator.start();
	}
}


class DriveForward implements Behavior
{

	private boolean _suppressed = false;

	public boolean takeControl()
	{
		return true;  // this behavior always wants control.
	}

	public void suppress()
	{
		_suppressed = true;// standard practice for suppress methods
	}

	public void action()
	{
		_suppressed = false;
		Motor.A.forward();
		Motor.C.forward();
		while (!_suppressed)
		{
			Thread.yield(); //don't exit till suppressed
		}
		Motor.A.stop(); // not strictly necessary, but good probramming practice
		Motor.C.stop();
	}
}


class DetectWall implements Behavior
{
	private TouchSensor touch;
	private UltrasonicSensor sonar;
	private int distance = 255;
	private boolean _suppressed = false;

	public DetectWall()
	{
		touch = new TouchSensor(SensorPort.S1);
		sonar = new UltrasonicSensor(SensorPort.S3);
		SensorThread sensorThread = new SensorThread();
		sensorThread.start();
	}

	public boolean takeControl()
	{
		return touch.isPressed() || (distance < 25);
	}

	public void suppress()
	{
		_suppressed = true;// standard practice for suppress methods
	}

	public void action()
	{
		boolean isDone = false;
		outer:while(!isDone) {
			_suppressed = false;
			Motor.A.backward();
			Motor.C.backward();
			long time = System.currentTimeMillis() + 1000;
			while(!_suppressed && System.currentTimeMillis() < time) {
				if (takeControl())
					continue outer;
			}
			Motor.A.stop();
			time = System.currentTimeMillis() + 800;
			while(!_suppressed && System.currentTimeMillis() < time) {
				if (takeControl())
					continue outer;
			}
			Motor.C.stop();
			isDone = true;
		}

		//Motor.A.rotate(-180, true);// start Motor.A rotating backward
		//Motor.C.rotate(-360);  // rotate C farther to make the turn
	}

	private class SensorThread extends Thread {

		public SensorThread() {
			this.setDaemon(true);
		}

		public void run() {
			while(true) {
				sonar.ping();
				Sound.pause(20);
				distance = sonar.getDistance();
			}
		}
	}
}

class Exit implements Behavior, ButtonListener
{
	private boolean _suppressed = false;
	private boolean _hasPressed = false;

	public Exit() {
		Button.ESCAPE.addButtonListener(this);
	}

	public boolean takeControl()
	{
		return _hasPressed;  // this behavior wants control if ESC is pressed
	}

	public void suppress()
	{
		_suppressed = true;// standard practice for suppress methods
	}

	public void action()
	{
		System.exit(0);
	}

	@Override
	public void buttonPressed(Button b) {
		_hasPressed = true;
	}

	@Override
	public void buttonReleased(Button b) {

	}
}
